﻿using System;
using System.Diagnostics;
using System.Net;
using AMEE.TcpComm;

namespace AMEE.BeinControl
{
    /// <summary>
    /// 
    /// </summary>
    public enum BeinState
    {
        /// <summary>No connection with the device.</summary>
        NotConnected = 0,
        /// <summary>Bein is initialized and didn't do anything yet.</summary>
        Initialized = 1,
        /// <summary>Command was send do Bein. Waiting for 'Done' Message.</summary>
        Working = 2,
        /// <summary>Command was executed. Ready to perform next command.</summary>
        Done = 3
    }

    /// <summary>
    /// Predefined leg commands for testing.
    /// </summary>
    /// <remarks>
    /// Hinweis: Die Befehle symbolisieren einzunehmende, vordefinierte Positionen, basierend auf dem Schrittmuster <i>Fast Walk</i>
    /// Sie sind NICHT kontextabhängig.
    /// Der Befehl GrabFront entspricht bspw einem angewinkelten Unterschenkel bei hängendem Oberschenkel - Fuß parallel zum Körper.
    /// Folgt daraufhin der Befehl <c>Raise</c> wird das Bein NICHT weitergehoben (das wäre kontextabhängig), sondern die damit definierte
    /// Position eingenommen: Ellenbogen nach hinten.
    /// </remarks>
    public enum BeinTestCommands
    {
        /// <summary>Command to take the step forward position (to Pos#1 = down ahead)</summary>
        Step,
        /// <summary>Command to pull the leg (to Pos#2 = down back)</summary>
        Pull,
        /// <summary>Command to raise the leg (to Pos#3 = up)</summary>
        Raise,
        /// <summary>Command to grab to front (to Pos#4 = up ahead)</summary>
        GrabFront,
        /// <summary>Command to send an Echo Message</summary>
        Echo,
        /// <summary>Command to take parking position (all legs bent to the body)</summary>
        Park,
        /// <summary>Command to send an Stop Message (Breaking all)</summary>
        Stop
    }

    /// <summary>
    /// 
    /// </summary>
    public class Bein : IDisposable
    {
        #region Fields

        bool _disposed = false;
        TcpWriter _client;
        int[] _upArr;
        int[] _upAheadArr;
        int[] _downAheadArr;
        int[] _downBackArr;

        #endregion

        /// <summary>
        /// Occurs when [state changed].
        /// </summary>
        public event EventHandler StateChanged;

        #region Properties

        /// <summary>
        /// Gets the IPV4 address.
        /// </summary>
        public String IPv4 { get { return Address.Address.ToString(); } }

        /// <summary>
        /// Gets the port.
        /// </summary>
        public int Port { get { return Address.Port; } }

        /// <summary>
        /// Gets the IPEndPoint address.
        /// </summary>
        public IPEndPoint Address { get; private set; }

        BeinState _state;
        /// <summary>
        /// Gets the state of the leg.
        /// </summary>
        public BeinState State {
            get { return _state; }
            private set {
                _state = value;
                this.RaiseStateChangedEvent();
            }
        }

        #endregion


        #region Constructor

        /// <summary>
        /// Initializes a new instance of the <see cref="Bein"/> class.<para>
        /// It is strongly recommended to perfom TEST_Set(Down/Up)Parameters before first command call!</para>
        /// </summary>
        /// <param name="ip">The ip.</param>
        /// <param name="port">The port.</param>
        /// <exception cref="AMEE.BeinControl.BeinControlException">Invalid constructor parameter.</exception>
        /// <remarks>
        /// State is <c>Initialized</c>, if TCP connection is established. State is <c>NotConnected</c> otherwise.
        /// </remarks>
        public Bein(string ip, int port) {
            _upArr = new int[3];
            _upAheadArr = new int[3];
            _downAheadArr = new int[3];
            _downBackArr = new int[3];
            initAddress(ip, port);
            bool connected = initCommunication();
            if (connected)
                State = BeinState.Initialized;
            else
                State = BeinState.NotConnected;
        }

        /// <summary>
        /// Finalizes an instance of the <see cref="Bein"/> class.
        /// </summary>
        ~Bein()
        {
            Dispose(false);
        }

        #endregion


        #region IDisposable Members

        /// <summary>
        /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources.
        /// </summary>
        public void Dispose() {
            Dispose(true);
            GC.SuppressFinalize(this);
        }

        #endregion


        #region Public

        /// <summary>
        /// Sends an 'Echo' command.
        /// </summary>
        public void Echo() {
            // Beinstatus Working?
            // send async mit ResponseReceived event
            State = BeinState.Working;
            var echoTask = new System.Threading.Tasks.Task(() => {
                System.Threading.Thread.Sleep(1000);
                _client.Send("$:E;");
            });
            echoTask.Start();
        }

        /// <summary>
        /// Does the specified command.<para>
        /// (Sends async the corresponding Command)</para>
        /// </summary>
        /// <param name="command">The command.</param>
        /// <returns>true, if command could send. The state is set to Working and no more commands will be accepted until State changed back to Done.<para>
        /// false, if the state is Working.</para></returns>
        public bool Do(BeinTestCommands command) {
            if (this.State == BeinState.Working)
                return false;
            Debug.WriteLineIf(!_client.Connected, "Bein.Do - Keine TCP Verbindung!", "Warning");
            switch (command) {
                case BeinTestCommands.Step:
                    _client.Send(Protocol.GetCommandMove2Point(_downAheadArr[0], _downAheadArr[1], _downAheadArr[2]));
                    Debug.WriteLine(String.Format("Bein sent to {0}:{1} \"{2}\"",this.IPv4, this.Port, Protocol.GetCommandMove2Point(_downAheadArr[0], _downAheadArr[1], _downAheadArr[2])),"Info");
                    State = BeinState.Working;
                    break;
                case BeinTestCommands.Pull:
                    _client.Send(Protocol.GetCommandMove2Point(_downBackArr[0], _downBackArr[1], _downBackArr[2]));
                    Debug.WriteLine(String.Format("Bein sent to {0}:{1} \"{2}\"", this.IPv4, this.Port, Protocol.GetCommandMove2Point(_downBackArr[0], _downBackArr[1], _downBackArr[2])), "Info");
                    State = BeinState.Working;
                    break;
                case BeinTestCommands.Raise:
                    _client.Send(Protocol.GetCommandMove2Point(_upArr[0], _upArr[1], _upArr[2]));
                    Debug.WriteLine(String.Format("Bein sent to {0}:{1} \"{2}\"", this.IPv4, this.Port, Protocol.GetCommandMove2Point(_upArr[0], _upArr[1], _upArr[2])), "Info");
                    this.State = BeinState.Working;
                    break;
                case BeinTestCommands.GrabFront:
                    _client.Send(Protocol.GetCommandMove2Point(_upAheadArr[0], _upAheadArr[1], _upAheadArr[2]));
                    Debug.WriteLine(String.Format("Bein sent to {0}:{1} \"{2}\"", this.IPv4, this.Port, Protocol.GetCommandMove2Point(_upAheadArr[0], _upAheadArr[1], _upAheadArr[2])), "Info");
                    State = BeinState.Working;
                    break;
                case BeinTestCommands.Echo:
                    this.Echo();
                    break;
                case BeinTestCommands.Park:
                    _client.Send(Protocol.CMD_ParkPosition);
                    Debug.WriteLine(String.Format("Bein sent to {0}:{1} \"{2}\"", this.IPv4, this.Port, Protocol.CMD_ParkPosition), "Info");
                    break;
                case BeinTestCommands.Stop:
                    // TODO Sonderfall - durch extra Methode behandeln und unabhängig vom aktuellen State.
                    break;
                default:
                    break;
            }
            return true;
        }

        
        /// <summary>
        /// Set the target vector of the Down Command (Pos#1 down ahead).<para>
        /// Parameters relative to the device's coord system.</para>
        /// </summary>
        /// <param name="x">x coordinate</param>
        /// <param name="y">y coordinate</param>
        /// <param name="z">z coordinate.</param>
        public void SetStepParameters(int x, int y, int z) {
            // Achtung!! Ohne Validierung
            _downAheadArr[0] = x;
            _downAheadArr[1] = y;
            _downAheadArr[2] = z;
        }
        /// <summary>
        /// Set the target vector of the Raise Command (Pos#3 up).<para>
        /// Parameters relative to the device's coord system.</para>
        /// </summary>
        /// <param name="x">x coordinate</param>
        /// <param name="y">y coordinate</param>
        /// <param name="z">z coordinate.</param>
        public void SetRaiseParameters(int x, int y, int z) {
            // Achtung!! Ohne Validierung
            _upArr[0] = x;
            _upArr[1] = y;
            _upArr[2] = z;
        }
        /// <summary>
        /// Set the target vector of the Pull Command (Pos#2 down back).<para>
        /// Parameters relative to the device's coord system.</para>
        /// </summary>
        /// <param name="x">x coordinate</param>
        /// <param name="y">y coordinate</param>
        /// <param name="z">z coordinate.</param>
        public void SetPullParameters(int x, int y, int z) {
            // Achtung!! Ohne Validierung
            _downBackArr[0] = x;
            _downBackArr[1] = y;
            _downBackArr[2] = z;
        }
        /// <summary>
        /// Set the target vector of the GrabFront Command (Pos#4 up ahead).<para>
        /// Parameters relative to the device's coord system.</para>
        /// </summary>
        /// <param name="x">x coordinate</param>
        /// <param name="y">y coordinate</param>
        /// <param name="z">z coordinate.</param>
        public void SetGrabFrontParameters(int x, int y, int z) {
            // Achtung!! Ohne Validierung
            _upAheadArr[0] = x;
            _upAheadArr[1] = y;
            _upAheadArr[2] = z;
        }

        ///// <summary>
        ///// Set the ... parameter.
        ///// </summary>
        ///// <param name="a">A.</param>
        ///// <param name="b">The b.</param>
        ///// <param name="c">The c.</param>
        ///// <param name="d">The d.</param>
        //public void TEST_SetUpParameters(int a, int b, int c, int d) {
        //    _upArr[0] = a;
        //    _upArr[1] = b;
        //    _upArr[2] = c;
        //    _upArr[3] = d;
        //}
        ///// <summary>
        ///// Set the ... parameter.
        ///// </summary>
        ///// <param name="a">A.</param>
        ///// <param name="b">The b.</param>
        ///// <param name="c">The c.</param>
        ///// <param name="d">The d.</param>
        //public void TEST_SetDownParameters(int a, int b, int c, int d) {
        //    _downArr[0] = a;
        //    _downArr[1] = b;
        //    _downArr[2] = c;
        //    _downArr[3] = d;
        //}

        #endregion

        /// <summary>
        /// Raises the state changed event.
        /// </summary>
        protected virtual void RaiseStateChangedEvent() {
            var stateChanged = this.StateChanged;
            if (stateChanged != null) {
                stateChanged(this, new EventArgs());
            }
        }


        #region Helper / Private

        /// <summary>
        /// Inits the address.
        /// </summary>
        /// <param name="ip">The ip.</param>
        /// <param name="port">The port.</param>
        /// <exception cref="AMEE.BeinControl.BeinControlException">Invalid constructor parameter</exception>
        void initAddress(string ip, int port) {
            try {
                IPAddress ipAdr = IPAddress.Parse(ip);
                Address = new IPEndPoint(ipAdr, port);
            }
            catch (Exception ex) {
                throw new BeinControlException("Invalid constructor parameter", ex);
            }
        }

        /// <summary>
        /// Inits the communication. Address have to be set!
        /// </summary>
        /// <returns>true, if connection established. false otherwise.</returns>
        bool initCommunication() {
            _client = new TcpWriter(IPv4, Port);
            Debug.WriteLineIf(!_client.Connected, "Bein init Comm failure. Couldn't connect to Server!", "Warning");
            _client.ResponseReceived += client_ResponseReceived;
            return _client.Connected;
        }

        void client_ResponseReceived(object sender, ResponseReceivedArg e) {
            String msg = e.Response;
            Debug.WriteLine(String.Format("Bein Port {0} received: \"{1}\"", this.Port, msg));
            if (msg.Contains("Done")) {
                this.State = BeinState.Done;
            }
            else {
                Debug.WriteLine("Bein.client_ResponseReceived - received message is not a 'Done' message. ", "Warning");
            }
        }

        /// <summary>
        /// Releases unmanaged and - optionally - managed resources.
        /// </summary>
        /// <param name="disposing"><c>true</c> to release both managed and unmanaged resources; <c>false</c> to release only unmanaged resources.</param>
        protected virtual void Dispose(bool disposing) {
            // Check to see if Dispose has already been called.
            if (!this._disposed) {
                // If disposing equals true, dispose all managed
                // and unmanaged resources.
                if (disposing) {
                    // Dispose managed resources.
                    if (_client != null)
                        _client.Close();
                    //if (_reader != null)
                    //    _reader.Dispose();
                    //if (_writer != null)
                    //    _writer.Dispose();
                }

                // Note disposing has been done.
                _disposed = true;

            }
        }

        #endregion
    }
}
